#include <math.h>
#include <stdlib.h>
#include "navigation.h"
#include "flamingdeath.h"

//#define HAVE_PRINT
#ifdef HAVE_PRINT
#include <stdio.h>
#define print printf
#else
//#include "usart.h"
//#define print xprintf
#endif


kal_t accelerometer_init() {
	// take a baseline measurement of the accelerometer
	
	kal_t baseLine[3] = {0,0,0};
	
	int change = accel_read_id;
	int last = change;
	int cnt = 0;
	
	while (accel_read_id < change + 1000) {
		
		while (accel_read_id == last);
		last = accel_read_id;
		baseLine[0] += (float)accel_x_raw * 0.75f;
		baseLine[1] += (float)accel_y_raw * 0.75f;
		baseLine[2] += (float)accel_z_raw * 0.75f;
		cnt++;
	}
	baseLine[0] /= cnt;
	baseLine[1] /= cnt;
	baseLine[2] /= cnt;
	
	return sqrtf(baseLine[0]*baseLine[0] + baseLine[1]*baseLine[1] + baseLine[2]*baseLine[2]);
}

void convert_accelerometer(kal_t orientation, kal_t *measured, 
						   kal_t *xy) {
	// isolate dynamic acceleration due to velocity change, i.e. not caused 
	// by gravity. with dynamic acceleration isolated, extract x and y components
	
	// orien		current orientation
	//				unchanged on exit
	// measured		current 3-axis measurement from accelerometer as
	//				{x,y,z}
	//				unchanged on exit
	// xy			overwritten with x and y components of accleration as
	//				{x,y}
	
	// 1) isolate acceleration due to movement
	//		a) find magnitude of measurement
	//		b) subtract off baseLine
	//		c) figure out if that is velocity change, or cornering
	
	static int baseLine = 1;
	kal_t magnitude = sqrtf(measured[0]*measured[0] + measured[1]*measured[1] + 
							measured[2]*measured[2]) - baseLine;
	
	// 2) extract x and y components
	
	// to incorporate any acceleration read from the acclerometers z-component
	// we must find the magnitude of acceleration in xy plane
		
	// if we never turned, we could assume this magniture is in the direction of 
	// travel (x) however in a turn, we should be observing acceleration 
	// perpendicular to travel (y).  
	
	// there is no way to know for certain the x and y components relative to 
	// direction of travel of the horizontal acceleration magnitude because
	// we cannot know what the z-axis measurement was reading.
	// I see a few options
	// 1) determine largest accelerometer reading and lump z value in that
	//			orientation
	// 2) split z value between x and y
	// 3) keep same angle between x and y and update x,y values with required
	//			share of z value.
	// 4) assume contant velocity in corners and apply complete magnitude to
	//			either front or front+90 of car.
	
	
	// going with option 4 for now.  we can try something more sophisticated if
	// this doesnt work.
	
	// were is the largest change?  in case of constant velocity
	// with no cornering make sure we apply the error to front/back
	kal_t lorientation = orientation;
	unsigned int x = abs((unsigned int)measured[0]);
	unsigned int y = abs((unsigned int)measured[1]);
	if (y > (x<<2)) {
		if (measured[1] < 0) 
			lorientation -= M_PI;
		else 
			lorientation += M_PI;
	}
	
	// travel /\  //
	//		  ||
	//
	//    -x
	// -y     y
	//     x
	// 
	// compass
	//     N
	//	W	  E
	//     S
	
	// *-1 for x-axis because of mounting position 
	xy[0] = -1 * extract_x_comp(lorientation,magnitude);
	xy[1] = extract_y_comp(lorientation,magnitude);
}



///////////////////////////// GPS ///////////////////////////

void gps_init(gps_t *gpsi) {
	gpsi->latitude = GPS_BAD;
	gpsi->new_latitude = GPS_BAD;
	gpsi->longitude = GPS_BAD;
	gpsi->new_longitude = GPS_BAD;
	
	gpsi->last_position_fix = GPS_BAD;
	gpsi->new_position_fix = GPS_BAD;
	
	gpsi->id = 0;
	gpsi->haveLock = 0;
	
	
	gpsi->parity = 0;
	gpsi->is_checksum_term = 0;
	gpsi->sentence_type = GPS_SENTENCE_OTHER;
	gpsi->term_number = 0;
	gpsi->term_offset = 0;
	gpsi->gps_data_good = 0;
	
	gpsi->term[0] = '\0';
	// this must match the size of term in the struct definition
	gpsi->termSize = 15;
}

#ifdef HAVE_PRINT
void gps_print(gps_t *gpsi) {
	printf("latitude: %f\t%ld\n",gpsi->latitude/ 100000.0,gpsi->latitude);
	printf("new_latitude: %f\t%ld\n",gpsi->new_latitude/ 100000.0,gpsi->new_latitude);
	printf("longitude: %f\t%ld\n",gpsi->longitude/ 100000.0,gpsi->longitude);
	printf("new_longitude: %f\t%ld\n",gpsi->new_longitude/ 100000.0,gpsi->new_longitude);
	printf("last_position_fix: %ld\n",gpsi->last_position_fix);
	printf("new_position_fix: %ld\n",gpsi->new_position_fix);
	
	printf("\n\n");
}
#endif

#include "flamingdeath.h"
uint8_t gps_encode(char c, gps_t *gpsi)
{
	uint8_t valid_sentence = 0;
	flamingdeath_BTSend(c);
	switch(c)
	{
		case ',': // term terminators
			gpsi->parity ^= c;
		case '\r':
		case '\n':
		case '*':
			if (gpsi->term_offset < gpsi->termSize-1)
			{
				gpsi->term[gpsi->term_offset] = 0;
				valid_sentence = term_complete(gpsi);
			}
			++(gpsi->term_number);
			gpsi->term_offset = 0;
			gpsi->is_checksum_term = c == '*';
			return valid_sentence;
			
		case '$': // sentence begin
			gpsi->term_number = gpsi->term_offset = 0;
			gpsi->parity = 0;
			gpsi->sentence_type = GPS_SENTENCE_OTHER;
			gpsi->is_checksum_term = 0;
			gpsi->gps_data_good = 0;
			return valid_sentence;
	}
	
	// ordinary characters
	if (gpsi->term_offset < gpsi->termSize - 1)
		gpsi->term[gpsi->term_offset++] = c;
	if (!gpsi->is_checksum_term)
		gpsi->parity ^= c;
	
	return valid_sentence;
}


// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
uint8_t term_complete(gps_t *gpsi)
{
	if (gpsi->is_checksum_term)
	{
		uint8_t checksum = 16 * from_hex(gpsi->term[0]) + from_hex(gpsi->term[1]);
		if (checksum == gpsi->parity)
		{
			if (gpsi->gps_data_good)
			{
				gpsi->last_position_fix = gpsi->new_position_fix;
				
				switch(gpsi->sentence_type)
				{
					case GPS_SENTENCE_GPRMC:
						gpsi->latitude  = gpsi->new_latitude;
						gpsi->longitude = gpsi->new_longitude;
						gpsi->course = gpsi->new_course;
						gpsi->speed = gpsi->new_speed;
						gpsi->haveLock = 1;
						break;
					//case GPS_SENTENCE_GPGGA:
					//	gpsi->latitude  = gpsi->new_latitude;
					//	gpsi->longitude = gpsi->new_longitude;
					//	break;
				}
				++(gpsi->id);
				return 1;
			}
		}
		return 0;
	}
	
	// the first term determines the sentence type
	if (gpsi->term_number == 0)
	{
		if (!gpsstrcmp(gpsi->term, GPRMC_TERM))
			gpsi->sentence_type = GPS_SENTENCE_GPRMC;
		//else if (!gpsstrcmp(gpsi->term, GPGGA_TERM))
		//	gpsi->sentence_type = GPS_SENTENCE_GPGGA;
		else {
			gpsi->sentence_type = GPS_SENTENCE_OTHER;
		}
		return 0;
	}
	
	if (gpsi->sentence_type != GPS_SENTENCE_OTHER && gpsi->term[0]) {
		//switch((gpsi->sentence_type == GPS_SENTENCE_GPGGA ? 200 : 100) + gpsi->term_number)
		switch(100+gpsi->term_number) {
			//case 101: // Time in both sentences
			//case 201:
				//gpsi->new_time = parse_decimal(gpsi);
				//_new_time_fix = get_curr_time();
			//	break;
			case 102: // GPRMC validity
				gpsi->gps_data_good = gpsi->term[0] == 'A';
				if (!gpsi->gps_data_good) {
					gpsi->haveLock = 0;
				}
				break;
			case 103: // Latitude
			//case 202:
				gpsi->new_latitude = parse_minutes(gpsi);
				gpsi->new_position_fix = get_curr_time();
				break;
			case 104: // N/S
			//case 203:
				if (gpsi->term[0] == 'S')
					gpsi->new_latitude = -(gpsi->new_latitude);
				break;
			case 105: // Longitude
			//case 204:
				gpsi->new_longitude = parse_minutes(gpsi);
				break;
			case 106: // E/W
			//case 205:
				if (gpsi->term[0] == 'W')
					gpsi->new_longitude = -(gpsi->new_longitude);
				break;
			case 107: // Speed (GPRMC)
				gpsi->new_speed = parse_speed(gpsi->term);
				break;
			case 108: // Course (GPRMC)
				gpsi->new_course = parse_radians(gpsi->term);
				break;
			//case 109: // Date (GPRMC)
				//_new_date = gpsatol(_term);
			//	break;
			//case 206: // Fix data (GPGGA)
			//	gpsi->gps_data_good = gpsi->term[0] > '0';
			//	break;
			//case 209: // Altitude (GPGGA)
				//_new_altitude = parse_decimal();
			//	break;
		}
	}
	
	return 0;
}

int from_hex(char a) {
	if (a >= 'A' && a <= 'F')
		return a - 'A' + 10;
	else if (a >= 'a' && a <= 'f')
		return a - 'a' + 10;
	else
		return a - '0';
}

unsigned long parse_minutes(gps_t *gpsi) {
	// convert lat or long to 10k minutes
	
	char *p = gpsi->term;
	unsigned long minutes = 0;
	
	// ddmm or dddmm
	while (gpsisdigit(*p))
		minutes = 10 * minutes + *p++ - '0';
	
	// decimal
	++p;
	
	// mmmm
	while (gpsisdigit(*p))
		minutes = 10 * minutes + *p++ - '0';
	
	// string ddmm.mmmm now as unsigned long ddmmmmmm
	unsigned long minOnly = minutes % 1000000;
	// extract dd000000 or ddd000000
	// divide by 1000000 to get degress
	// multiply by 60 to get minutes
	// multiply by 10000 to get 10k minutes
	// reduces to dd*6/10
	unsigned long degreesAsMinutes = ((minutes - minOnly)*6)/10;
	
	// sum degree minutes to minutes
	return degreesAsMinutes+minOnly;
	
}


long gpsatol(const char *str) {
	long ret = 0;
	while (gpsisdigit(*str))
		ret = 10 * ret + *str++ - '0';
	return ret;
}

int gpsstrcmp(const char *str1, const char *str2) {
	while (*str1 && *str1 == *str2)
		++str1, ++str2;
	return *str1;
}

uint8_t gpsisdigit(char c) { 
	return c >= '0' && c <= '9'; 
}

float parse_decimal(char *term) {
	// parse x.x
	char *p = term;
	uint8_t isneg = *p == '-';
	if (isneg) ++p;
	unsigned long retL = 0;
	
	while (gpsisdigit(*p))
		retL = 10 * retL + *p++ - '0';
	
	// decimal
	++p;
	
	//[0-9]
	unsigned long retR = 0;
	int cnt = 1;
	while (gpsisdigit(*p)) {
		retR = 10 * retR + *p++ - '0';
		cnt *= 10;
	}
	
	float ret = (float)retL + ((float)retR / (float)cnt);
	
	return isneg ? -ret : ret;
}

float parse_speed(char *term) {
	// parse degress
	float ret = parse_decimal(term);
	
	// to meters/second
	ret *= 0.5144444f;
	
	return ret;
}

float parse_radians(char *term) {
	// parse degress
	float ret = parse_decimal(term);
	
	// to radians
	ret *= M_PI/180.0f;
	
	return ret;
}

unsigned long get_curr_time() {
	return 0;
}

